// These are the functions that involved human control

int limitMoveSpeed(int position) { // Limits and scales drive move speed
  int output = position*maxDrivePower/127;
  if (abs(output) > maxDrivePower) {
    if (output < 0) {
      return -maxDrivePower;
    }
    else {
      return maxDrivePower;
    }
  }
  else {
    return output;
  }
}

void tankDrive() // Two joystick Drive
{
  int wheelLeftPowertmp; int wheelRightPowertmp;
  if (abs(vexRT[Ch3]) > joystickThreshold)
  {
    wheelLeftPowertmp = limitMoveSpeed(vexRT[Ch3]);
  }
  else if (!debugMode)
  {
    wheelLeftPowertmp = 0;
  }
  if (abs(vexRT[Ch4])>bookThreshold) { // if the joystick is tilted horizontally past a certain threshold...
    if (vexRT[Ch4] < 0) {
      wheelLeftPowertmp = -bookDrivePower; // ...activate "book-it" mode (near max speed)
    }
    else {
      wheelLeftPowertmp = bookDrivePower;
    }
  }
  if (abs(vexRT[Ch2]) > joystickThreshold)
  {
    wheelRightPowertmp = limitMoveSpeed(vexRT[Ch2]);
  }
  else if (!debugMode)
  {
    wheelRightPowertmp = 0;
  }
  if (abs(vexRT[Ch1])>bookThreshold) {
    if (vexRT[Ch1] < 0) {
      wheelRightPowertmp = bookDrivePower;
    }
    else {
      wheelRightPowertmp = -bookDrivePower;
    }
  }
  if (abs(wheelLeftPowertmp) > 0 || abs(wheelRightPowertmp) > 0) {
    driveState = 3;
    //turn_ticks = 0;
    //move_dist = 0;
    //moveActivate = false;
	}
	if (driveState == 3) {
	  wheelLeftPower = wheelLeftPowertmp;
	  wheelRightPower = wheelRightPowertmp;
	}
}

/*void accelDrive() // Accelerometer Drive (not precise)
{
  prevx = accx;
  accx = vexRT[AccelX];

  if (vexRT[Btn7L] == 1)
  {
    if (abs(accx) < accelThreshold)
    {
      wheelLeftPower = maxMoveSpeed;
      wheelRightPower = maxMoveSpeed;
    }
    else if (abs(prevx - accx) < accelThreshold)
    {
      wheelRightPower = (maxMoveSpeed-accx)/2;
      wheelLeftPower = (maxMoveSpeed+accx)/2;
    }
  }
  else if (vexRT[Btn7D] == 1)
  {
    if (abs(accx) < accelThreshold)
    {
      wheelLeftPower = -maxMoveSpeed;
      wheelRightPower = -maxMoveSpeed;
    }
    else if (abs(prevx - accx) < accelThreshold)
    {
      wheelRightPower = (-maxMoveSpeed+accx)/2;
      wheelLeftPower = (-maxMoveSpeed-accx)/2;
    }
  }
  else
  {
    wheelLeftPower = 0;
    wheelRightPower = 0;
  }
}*/

/*--------------------------Main User Controls----------------------------*/
void useControls(int controlNumber) // Uses controls depending on controlSchemeNumber
{
  if (controlNumber == 1)
  {
    tankDrive();
  }
  /*else if (controlNumber == 2 && driveState == 3)
  {
    accelDrive(); // accel drive not used
  }*/

  // Arm Movement:
  if (vexRT[Btn5U] == 1) // Arm Up - 5U
  {
    if (armState == 6)
    {
      armAction = 0;
      armLeftPower = armUpPower;
      armRightPower = armUpPower;
    }
    else if (btn5u_isPressed == 0)
    {
      changeArmState(1);
    }
  }
  else if (vexRT[Btn5D] == 1) // Arm Down - 5D
  {
    if (armState == 6)
    {
      armAction = 0;
      armLeftPower = -armDownPower;
      armRightPower = -armDownPower;
    }
    else if (btn5d_isPressed == 0)
    {
      changeArmState(0);
    }
  }
  else if (armAction != 1 && !debugMode)
  {
    armLeftPower = 0;
    armRightPower = 0;
  }

  // Claw:
  if (vexRT[Btn6U] == 1) // Claw Open - 6U
  {
    if (btn6u_isPressed == 0) // Auto-open
    {
      clawState = 4;
    }
    else if (vexRT[Btn8D]==1)
    {
      clawState = 3;
      clawPower = clawOpenPower;
    }
  }
  else if (vexRT[Btn6D] == 1) // Claw Close - 6D
  {
    clawState = 3;
    clawPower = -clawClosePower;
  }
  else if (clawState == 3)
  {
    clawPower = 0;
  }
}

/*void changeControlScheme() // Change controlSchemeNumber, thus changing control scheme
{
  controlSchemeNumber ++;
  if (controlSchemeNumber >= 3)
  {
    controlSchemeNumber = 1;
  }
}*/
